Voronoi Graphs

Voronoi diagrams can be tricky to construct, but thankfully Python's SciPy library already has an implementation of the Voronoi method built in! The way this method works is that you need to feed in a set of points that, in our case, represent the obstacles, and create a Voronoi() object that contains a graph of the "ridges" that define the midline in free space between the obstacles.

For example, you can generate some random points and plot up the associated Voronoi graph like this:

# Import numpy and Voronoi method and plotting routines
import numpy as np
from scipy.spatial import Voronoi, voronoi_plot_2d
import matplotlib.pyplot as plt

# Generate 50 random points with integer values between 0 and 49
points = np.random.randint(50, size=(50, 2))
# Extract the Voronoi diagram
graph = Voronoi(points)
# Plot it up!
voronoi_plot_2d(graph)
plt.show()

And you get something that looks like this (will look different if you try this at home because values are random):

In this image, you can think of the blue points as obstacles (inputs to the Voronoi method), and orange points are the intersections of the Voronoi ridges, or in other words, the nodes in a graph that navigates between the obstacles!

In the real world, of course, obstacles are not points but they have some size and shape. Still, the Voronoi method can be a useful starting point for generating a planning graph around real world obstacles and that's what we're going to do next!

Extracting Nodes and Edges

For the example shown above, the edges in the graph or "ridges" as they're often called in the context of Voronoi diagrams, all represent feasible paths to navigate around those point obstacles. In the upcoming exercise, you'll start by creating a similar graph based on obstacle center points, but then you need to add an additional step to test whether edges in the graph are traversing free or infeasible space.

Within the graph object created in the code snippet above, you've got all the information you need to extract nodes and edges and test for feasibility. The graph.vertices attribute contains the x and y positions of each of the vertices (or nodes), and graph.ridge_vertices contains the indices that identify each pair of points that define a ridge in the graph.vertices list. I think that will make more sense if I say it in code!

# Assuming you've created a graph object as shown above
# You can step through each pair of points that define a ridge like this
edges = []  # Empty list to contain valid edges
for v in graph.ridge_vertices:
    p1 = graph.vertices[v[0]]
    p2 = graph.vertices[v[1]]

    # Then you can test each pair p1 and p2 for collision using Bresenham
    # (need to convert to integer if using prebuilt Python package)
    # If the edge does not hit an obstacle
    # add it to the list
    if not in_collision:
        edges.append((p1, p2))

Your goal is to plot up all the valid edges and the result should look something like this:

Voronoi Exercise

In the notebook below, you'll read in the map of the world that you've been using in previous exercises and generate a Voronoi graph like the one above for obstacles that are not simple points, but actual 2D polygons in the ground plane. To accomplish this task, you'll need to fill in the TODOs in the create_grid_and_edges() function below.

Good luck! And for a peek at our solution you can scroll down to the link at the bottom of the notebook.